/*
 * cell_locator.cpp
 *
 *  Created on: 31.7.2012
 *      Author: stebel
 */

#include <limits>
#include <stdio.h>
#include <math.h>
#include "cell_locator.h"
#include "triboxoverlap.h"

#include "fem.h"
#include <petsc.h>


/** Tolerance for identification of adjacent faces */
#define TOL_FACE_DISTANCE 1e-1


double dot(double a[3], double b[3])
{
	return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
}

bool CellLocator::point_in_triangle(double x, double y, double z, int e, double &u, double &v)
{
	// Compute vectors
	double v0[3], v1[3], v2[3];
	for (int i=0; i<3; i++)
	{
		v0[i] = mesh->getCoord(mesh->getFaceNode(e, 2), i).getVal() - mesh->getCoord(mesh->getFaceNode(e, 0), i).getVal();
		v1[i] = mesh->getCoord(mesh->getFaceNode(e, 1), i).getVal() - mesh->getCoord(mesh->getFaceNode(e, 0), i).getVal();
		v2[i] = (i==0?x:(i==1?y:z)) - mesh->getCoord(mesh->getFaceNode(e, 0), i).getVal();
	}

	// Compute dot products
	double dot00 = dot(v0, v0);
	double dot01 = dot(v0, v1);
	double dot02 = dot(v0, v2);
	double dot11 = dot(v1, v1);
	double dot12 = dot(v1, v2);

	// Compute barycentric coordinates
	double invDenom = 1 / (dot00 * dot11 - dot01 * dot01);
	v = (dot11 * dot02 - dot01 * dot12) * invDenom;
	u = (dot00 * dot12 - dot01 * dot02) * invDenom;

	// Check if point is in triangle
	if ((u >= 0) && (v >= 0) && (u + v <= 1))
	{
		// compute distance of the point from the triangle's plane
		double n[3];

		n[0] = v0[1]*v1[2] - v0[2]*v1[1];
		n[1] = v0[2]*v1[0] - v0[0]*v1[2];
		n[2] = v0[0]*v1[1] - v0[1]*v1[0];
		double nsize = sqrt(n[0]*n[0]+n[1]*n[1]+n[2]*n[2]);
		n[0] /= nsize;
		n[1] /= nsize;
		n[2] /= nsize;

		double distance = n[0]*(x-mesh->getCoord(mesh->getFaceNode(e, 0), 0).getVal())
						 +n[1]*(y-mesh->getCoord(mesh->getFaceNode(e, 0), 1).getVal())
						 +n[2]*(z-mesh->getCoord(mesh->getFaceNode(e, 0), 2).getVal());

		if (fabs(distance) < TOL_FACE_DISTANCE) return true;
	}

	return false;
}

CellLocator::CellLocator(Mesh* _mesh, int _surface, int _res_x, int _res_y, int _res_z)
	: mesh(_mesh),
	  surface(_surface)
{
	// initialize bounds
	for (int i=0; i<3; i++)
	{
		bounds[i][0] = numeric_limits<double>::max();
		bounds[i][1] = -numeric_limits<double>::max();
	}

	// find the bounding box
	for (int e=0; e<mesh->getNfaces(); e++)
	{
		if (mesh->getFacePhysicalNo(e) == surface)
		{
			for (int i=0; i<mesh->getNfaceDofs(); i++)
			{
				int nid = mesh->getFaceNode(e, i);
				for (int c=0; c<3; c++)
				{
					double coord = mesh->getCoord(nid, c).getVal();
					if (coord > bounds[c][1]) bounds[c][1] = coord;
					if (coord < bounds[c][0]) bounds[c][0] = coord;
				}
			}
		}
	}

	// set box size
	resolution[0] = _res_x;
	resolution[1] = _res_y;
	resolution[2] = _res_z;
	for (int i=0; i<3; i++)
	{
		sizes[i] = (bounds[i][1]-bounds[i][0]) / resolution[i];
		if (sizes[i] < 2*TOL_FACE_DISTANCE)
		{
			bounds[i][0] -= TOL_FACE_DISTANCE;
			bounds[i][1] += TOL_FACE_DISTANCE;
			sizes[i] = 2*TOL_FACE_DISTANCE;
		}
	}

	// setup box-to-face information
	for (int e=0; e<mesh->getNfaces(); e++)
	{
		if (mesh->getFacePhysicalNo(e) != surface) continue;
		// first find bounding box for the triangle
		double tbox[3][2];
		for (int i=0; i<3; i++)
		{
			tbox[i][0] = numeric_limits<double>::max();
			tbox[i][1] = -numeric_limits<double>::max();
		}
		for (int i=0; i<mesh->getNfaceDofs(); i++)
		{
			int nid = mesh->getFaceNode(e, i);
			for (int c=0; c<3; c++)
			{
				double coord = mesh->getCoord(nid, c).getVal();
				if (coord > tbox[c][1]) tbox[c][1] = coord;
				if (coord < tbox[c][0]) tbox[c][0] = coord;
			}
		}

		// now go through all boxes intersecting the bounding box
		// and test whether they coinside with the triangle
		int bid0 = box_id_by_point(tbox[0][0], tbox[1][0], tbox[2][0]);
		int nx = box_id_by_point(tbox[0][1], tbox[1][0], tbox[2][0]) - bid0 + 1,
			ny = (box_id_by_point(tbox[0][0], tbox[1][1], tbox[2][0]) - bid0) / resolution[0] + 1,
			nz = (box_id_by_point(tbox[0][0], tbox[1][0], tbox[2][1]) - bid0) / resolution[1] + 1;
		double triverts[3][3];
		for (int i=0; i<3; i++)
			for (int j=0; j<3; j++)
				triverts[i][j] = mesh->getCoord(mesh->getFaceNode(e, i), j).getVal();
		for (int ix = 0; ix < nx; ix++)
		{
			for (int iy = 0; iy<ny; iy++)
			{
				for (int iz = 0; iz<nz; iz++)
				{
					double bcenter[3];
					bcenter[0] = bounds[0][0] + (bid0%resolution[0] + 0.5 + ix)*sizes[0];
					bcenter[1] = bounds[1][0] + (bid0/resolution[0] + 0.5 + iy)*sizes[1];
					bcenter[2] = bounds[2][0] + (bid0/(resolution[0]*resolution[1]) + 0.5 + iz)*sizes[2];

					double bhalfsize[3];
					for (int i=0; i<3; i++) bhalfsize[i] = sizes[i]*0.5;

					if (triBoxOverlap(bcenter, bhalfsize, triverts) == 1)
						box_faces[bid0 + ix + resolution[0]*(iy + resolution[1]*iz)].insert(e);
				}
			}
		}
	}
}

CellLocator::~CellLocator()
{
}

int CellLocator::find_cell(double x, double y, double z, double &u, double &v)
{
	int bid = box_id_by_point(x, y, z);

	if (bid < 0 || bid >= resolution[0]*resolution[1]*resolution[2])
	{
		printf("Box id %d out of range.\n", bid);
		return -1;
	}

	for (set<int>::iterator e=box_faces[bid].begin(); e!=box_faces[bid].end(); e++)
	{
		if (point_in_triangle(x, y, z, *e, u, v)) return *e;
	}

	return -1;
}

int CellLocator::box_id_by_point(double x, double y, double z)
{
	int res = ((min(resolution[2]-1, (int)floor((z-bounds[2][0])/sizes[2]))*resolution[1]
	           +min(resolution[1]-1, (int)floor((y-bounds[1][0])/sizes[1]))*resolution[0])
               +min(resolution[0]-1, (int)floor((x-bounds[0][0])/sizes[0])));

	return res;
}


